﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Research.Kinect.Nui;

namespace KinectGestureRecognizer.Helper
{
    public static class helperFunctions
    {
        public static Vector3 vectorToVector3(Vector vector)
        {
            return new Vector3(vector.X, vector.Y, vector.Z);
        }

        public static float Vector3XDistance(Vector3 value1, Vector3 value2)
        {
            return Math.Abs(value1.X - value2.X);
        }

        public static float Vector3YDistance(Vector3 value1, Vector3 value2)
        {
            return Math.Abs(value1.Y - value2.Y);
        }

        /// <summary>
        /// Converte un Vector rappresentante una posizione 3D del kinect in una posizione sullo schermo. La posizione verrà
        /// centrata rispetto alla ViewPort.
        /// </summary>
        /// <param name="Position">Il Vector rappresentante la posizione.</param>
        /// <param name="scalePixel">La "scala" della conversione. Ovvero quanti Pixel dovranno corrispondere ad un metro.</param>
        /// <returns></returns>
        public static Vector2 screenizeKinectPosition(Vector Position, Game game, int scalePixel)
        {
            var X = Position.X;
            var Y = Position.Y;

            return new Vector2(X * scalePixel + game.GraphicsDevice.Viewport.Width / 2,
                               -Y * scalePixel + game.GraphicsDevice.Viewport.Height / 2);
        }

        public static float angleBetween2Points(Vector3 point1, Vector3 point2, int PLANE)
        {
            double theta;
            double x = 0;
            double y = 0;

            switch (PLANE)
            {
                case 0:
                    x = point2.X - point1.X;
                    y = point2.Y - point1.Y;
                    break;
                case 1:
                    x = point2.X - point1.X;
                    y = point2.Z - point1.Z;
                    break;
                case 2:
                    x = point2.Y - point1.Y;
                    y = point2.Z - point1.Z;
                    break;
            }

            if (Math.Abs(x) <= 0)
                if (Math.Abs(y) < 0)
                    theta = 0;
                else if (y > 0)
                    theta = Math.PI / 2;
                else
                    theta = -Math.PI / 2;
            else
            {
                theta = Math.Atan(y / x);

                if (x < 0)
                    if (y < 0)
                        theta += Math.PI;
                    else
                        theta -= Math.PI;
            }

            return (float)theta;
        }
    }
}
